//
//  Triangle.m
//  viewTest
//
//  Created by seonwoo Kang on 2014. 1. 15..
//  Copyright (c) 2014년 seonwoo Kang. All rights reserved.
//

#import "Triangle.h"

@implementation Triangle
- (BeaconData*) calculatePosition:(BeaconData*) b1 secondbalance:(BeaconData *)b2 thirdbalance:(BeaconData *)b3
{
    double a[2][2];
    double b[2][1];
    BeaconData *result = [[BeaconData alloc] init];
    BeaconData *beacon[3];
    beacon[0] = [[BeaconData alloc] init];
    beacon[1] = [[BeaconData alloc] init];
    beacon[2] = [[BeaconData alloc] init];
    
    beacon[0] = b1;
    beacon[1] = b2;
    beacon[2] = b3;
    
    a[0][0] = 2*([beacon[0] getX] - [beacon[1] getX]);
    a[0][1] = 2*([beacon[0] getY] - [beacon[1] getY]);
    a[1][0] = 2*([beacon[2] getX] - [beacon[0] getX]);
    a[1][1] = 2*([beacon[2] getY] - [beacon[0] getY]);
    
    int D = 1;
    
    b[0][0] = ( ([beacon[0] getDistance] * [beacon[0] getDistance])*D - ([beacon[2] getDistance] * [beacon[2] getDistance])*D ) / (2*([beacon[0] getY] - [beacon[1] getY]));
    
    
    b[0][0] = ([beacon[1] getDistance] * [beacon[1] getDistance]*D - ([beacon[0] getDistance] * [beacon[0] getDistance])*D
    - ([beacon[1] getX] * [beacon[1] getX]) + ([beacon[0] getX] * [beacon[0] getX])
    - ([beacon[1] getY] * [beacon[1] getY]) + ([beacon[0] getY] * [beacon[0] getY]));
               
    b[1][0] = ([beacon[0] getDistance] * [beacon[0] getDistance])*D - ([beacon[2] getDistance] * [beacon[2] getDistance])*D
    - ([beacon[0] getX] * [beacon[0] getX]) + ([beacon[2] getX] * [beacon[2] getX])
    - ([beacon[0] getY] * [beacon[0] getY]) + ([beacon[2] getY] * [beacon[2] getY]);

    
    NSLog(@"result xy : %.2f %.2f %.2f %.2f %.2f %.2f ",[beacon[0] getX],[beacon[0] getY],[beacon[1] getX],[beacon[1] getY],[beacon[2] getX] , [beacon[2] getY]);
    
    if([beacon[0] getX] == [beacon[1] getX] || [beacon[2] getX] == [beacon[1] getX] || [beacon[0] getX] == [beacon[3] getX])
    {
        [result setX: (a[1][1]*b[0][0] - a[0][1]*b[1][0])/(a[0][0]*a[1][1] - a[1][0]*a[0][1])];

        if([beacon[0] getX] == [beacon[1] getX])
        {
            [result setY: ( ([beacon[0] getDistance] * [beacon[0] getDistance])*D - ([beacon[1] getDistance] * [beacon[1] getDistance])*D ) / (2*([beacon[1] getY] - [beacon[0] getY]))];
            NSLog(@"11: result2 : %.2f %.2f %.2f %.2f %.2f %.2f %.2f ",[beacon[0] getDistance],[beacon[1] getDistance],[beacon[2] getDistance],[beacon[0] getY],[beacon[1] getY] , [result getX],[result getY]);
        }
        if([beacon[1] getX] == [beacon[2] getX])
        {
            [result setY: ( ([beacon[1] getDistance] * [beacon[1] getDistance])*D - ([beacon[2] getDistance] * [beacon[2] getDistance])*D ) / (2*([beacon[2] getY] - [beacon[1] getY]))];
            NSLog(@"12: result2 : %.2f %.2f %.2f %.2f %.2f %.2f %.2f ",[beacon[0] getDistance],[beacon[1] getDistance],[beacon[2] getDistance],[beacon[1] getY],[beacon[2] getY] , [result getX],[result getY]);
        }
        if([beacon[2] getX] == [beacon[0] getX])
        {
            [result setY: ( ([beacon[2] getDistance] * [beacon[2] getDistance])*D - ([beacon[0] getDistance] * [beacon[0] getDistance])*D ) / (2*([beacon[0] getY] - [beacon[2] getY]))];
            NSLog(@"13: result2 : %.2f %.2f %.2f %.2f %.2f %.2f %.2f ",[beacon[0] getDistance],[beacon[1] getDistance],[beacon[2] getDistance],[beacon[2] getY],[beacon[0] getY] , [result getX],[result getY]);
        }
        

    }
    else if([beacon[0] getY] == [beacon[1] getY] || [beacon[2] getY] == [beacon[1] getY] || [beacon[0] getY] == [beacon[3] getY])
    {
        [result setY: (-a[1][0]*b[0][0] + a[0][0]*b[1][0])/(a[0][0]*a[1][1] - a[0][1]*a[1][0])];

        if([beacon[0] getY] == [beacon[1] getY])
        {
            [result setX: ( ([beacon[0] getDistance] * [beacon[0] getDistance])*D - ([beacon[1] getDistance] * [beacon[1] getDistance])*D ) / (2*([beacon[1] getX] - [beacon[0] getX]))];
            NSLog(@"21: result2 : %.2f %.2f %.2f %.2f %.2f %.2f %.2f ",[beacon[0] getDistance],[beacon[1] getDistance],[beacon[2] getDistance],[beacon[0] getY],[beacon[1] getY] , [result getX],[result getY]);
        }
        if([beacon[1] getY] == [beacon[2] getY])
        {
            [result setX: ( ([beacon[1] getDistance] * [beacon[1] getDistance])*D - ([beacon[2] getDistance] * [beacon[2] getDistance])*D ) / (2*([beacon[2] getX] - [beacon[1] getX]))];
            NSLog(@"22: result2 : %.2f %.2f %.2f %.2f %.2f %.2f %.2f ",[beacon[0] getDistance],[beacon[1] getDistance],[beacon[2] getDistance],[beacon[1] getY],[beacon[2] getY] , [result getX],[result getY]);
        }
        if([beacon[2] getY] == [beacon[0] getY])
        {
            [result setX: ( ([beacon[2] getDistance] * [beacon[2] getDistance])*D - ([beacon[0] getDistance] * [beacon[0] getDistance])*D ) / (2*([beacon[0] getX] - [beacon[2] getX]))];
            NSLog(@"21: result2 : %.2f %.2f %.2f %.2f %.2f %.2f %.2f ",[beacon[0] getDistance],[beacon[1] getDistance],[beacon[2] getDistance],[beacon[2] getY],[beacon[0] getY] , [result getX],[result getY]);
        }
    }
    else{
        [result setX: (a[1][1]*b[0][0] - a[0][1]*b[1][0])/(a[0][0]*a[1][1] - a[1][0]*a[0][1])];
        [result setY: (-a[1][0]*b[0][0] + a[0][0]*b[1][0])/(a[0][0]*a[1][1] - a[0][1]*a[1][0])];
        NSLog(@"3: result2 : %.2f %.2f %.2f %.2f %.2f ",[beacon[0] getDistance],[beacon[1] getDistance],[beacon[2] getDistance], [result getX],[result getY]);
    }
    return result;
}

- (BeaconData*) calculatePositions:(NSArray*)beaconsArr
{
    BeaconData * beaconResult = [[BeaconData alloc]init];
    BeaconData * beaconTemp = [[BeaconData alloc]init];
    //beaconsArry = beaconsArr;
    
    //NSArray * beaconCalArr [10] = [[NSArray alloc]init];
    NSMutableArray * beaconCalArr = [[NSMutableArray alloc] init];
    
    float avgX = 0;
    float avgY = 0;
    
    
    int k = 0;
    
    
//    if([[[beaconsArr objectAtIndex:i] major] intValue] == 64328)//mint
//    {
//        NSLog(@"mint : %f", [self.beacon1.distance floatValue]);
//    }
//    else if([[[beacons objectAtIndex:i] major] intValue] == 23162)//blue
//    {
//        NSLog(@"blue  : %f", [self.beacon1.distance floatValue]);
//    }
//    else if([[[beacons objectAtIndex:i] major] intValue] == 18020)// purple
//    {
//        NSLog(@"purple  : %f", [self.beacon1.distance floatValue]);
//    }
//    else if([[[beacons objectAtIndex:i] major] intValue] == 64102)// mint2
//    {
//        NSLog(@"mint2  : %f", [self.beacon1.distance floatValue]);
//    }
//    else if([[[beacons objectAtIndex:i] major] intValue] == 46531) //purple2
//    {
//        NSLog(@"purple2  : %f", [self.beacon1.distance floatValue]);
//    }
//    else if([[[beacons objectAtIndex:i] major] intValue] == 21014) // blue2
//    {
//        NSLog(@"blue2  : %f", [self.beacon1.distance floatValue]);
//    }
    for (int i=2; i< [beaconsArr count]; i++) {
        for (int j=0; j<i; j++) {
            for (int j2 = 0; j2<j; j2++) {
                //NSLog(@"%d , %d, %d",i,j,j2);
                if(([beaconsArr[i] getX] == [beaconsArr[j] getX] && [beaconsArr[i] getX] == [beaconsArr[j2] getX]) || ([beaconsArr[i] getY] == [beaconsArr[j] getY] && [beaconsArr[i] getY] == [beaconsArr[j2] getY]))
                    continue;
                [beaconCalArr addObject:(BeaconData*) [self calculatePosition:beaconsArr[i] secondbalance:beaconsArr[j] thirdbalance:beaconsArr[j2]]];
                
                k++;
            }
        }
    }
    
    for (int i=0; i< k; i++) {
        beaconTemp = [beaconCalArr objectAtIndex:i];
        avgX += [beaconTemp getX];
        avgY += [beaconTemp getY];
    }
    
    [beaconResult setX:(avgX/(float)k)];
    [beaconResult setY:(avgY/(float)k)];
    
    return beaconResult;
}

@end